
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_backend.h
  * @author     baiyang
  * @date       2021-10-11
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <rtthread.h>

#include <common/time/gp_time.h>
#include <common/gp_rotations.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
enum DevTypes {
    DEVTYPE_BMI160       = 0x09,
    DEVTYPE_L3G4200D     = 0x10,
    DEVTYPE_ACC_LSM303D  = 0x11,
    DEVTYPE_ACC_BMA180   = 0x12,
    DEVTYPE_ACC_MPU6000  = 0x13,
    DEVTYPE_ACC_MPU9250  = 0x16,
    DEVTYPE_ACC_IIS328DQ = 0x17,
    DEVTYPE_ACC_LSM9DS1  = 0x18,
    DEVTYPE_GYR_MPU6000  = 0x21,
    DEVTYPE_GYR_L3GD20   = 0x22,
    DEVTYPE_GYR_MPU9250  = 0x24,
    DEVTYPE_GYR_I3G4250D = 0x25,
    DEVTYPE_GYR_LSM9DS1  = 0x26,
    DEVTYPE_INS_ICM20789 = 0x27,
    DEVTYPE_INS_ICM20689 = 0x28,
    DEVTYPE_INS_BMI055   = 0x29,
    DEVTYPE_SITL         = 0x2A,
    DEVTYPE_INS_BMI088   = 0x2B,
    DEVTYPE_INS_ICM20948 = 0x2C,
    DEVTYPE_INS_ICM20648 = 0x2D,
    DEVTYPE_INS_ICM20649 = 0x2E,
    DEVTYPE_INS_ICM20602 = 0x2F,
    DEVTYPE_INS_ICM20601 = 0x30,
    DEVTYPE_INS_ADIS1647X = 0x31,
    DEVTYPE_SERIAL       = 0x32,
    DEVTYPE_INS_ICM40609 = 0x33,
    DEVTYPE_INS_ICM42688 = 0x34,
    DEVTYPE_INS_ICM42605 = 0x35,
};

/**
  * @brief       陀螺仪、加速度计驱动的基础结构体
  * @param[out]  
  * @retval      
  * @note        
  */
typedef struct {
    struct sensor_imu_backend_ops *ops;

    // semaphore for access to shared frontend data
    rt_sem_t _sem;

    //Default Clip Limit
    float _clip_limit;     // default 15.5f * GRAVITY_MSS;

    // backend unique identifier or -1 if backend doesn't identify itself
    int16_t _id;           // default -1

    // support for updating filter at runtime
    uint16_t _last_accel_filter_hz;
    uint16_t _last_gyro_filter_hz;
    float _last_notch_center_freq_hz;
    float _last_notch_bandwidth_hz;
    float _last_notch_attenuation_dB;

    // support for updating harmonic filter at runtime
    float _last_harmonic_notch_center_freq_hz;
    float _last_harmonic_notch_bandwidth_hz;
    float _last_harmonic_notch_attenuation_dB;
} sensor_imu_backend;

/**
 * uart operators
 */
struct sensor_imu_backend_ops {
    void (*sensor_imu_backend_destructor)(sensor_imu_backend *imu_backend);

    /*
     * Update the sensor data. Called by the frontend to transfer
     * accumulated sensor readings to the frontend structure via the
     * _publish_gyro() and _publish_accel() functions
     */
    bool (*update)(sensor_imu_backend *imu_backend);
    
    /*
     * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
     */
    void (*accumulate)(sensor_imu_backend *imu_backend);

    /*
     * Configure and start all sensors. The empty implementation allows
     * subclasses to already start the sensors when it's detected
     */
    void (*start)(sensor_imu_backend *imu_backend);

    // get a startup banner to output to the GCS
    bool (*get_output_banner)(sensor_imu_backend *imu_backend, char* banner, uint8_t banner_len);
};

/*----------------------------------variable----------------------------------*/
extern struct sensor_imu_backend_ops imu_backend_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_backend_ctor(sensor_imu_backend *imu_backend, char *name);

static inline void sensor_imu_backend_destructor(sensor_imu_backend *imu_backend) {
    if (imu_backend->ops->sensor_imu_backend_destructor != NULL) {
        imu_backend->ops->sensor_imu_backend_destructor(imu_backend);
    }
}

/*
 * Update the sensor data. Called by the frontend to transfer
 * accumulated sensor readings to the frontend structure via the
 * _publish_gyro() and _publish_accel() functions
 */
static inline bool sensor_imu_backend_update(sensor_imu_backend *imu_backend) {
    if (imu_backend->ops->update == NULL) {
        return false;
    }

    return imu_backend->ops->update(imu_backend);
}

/*
 * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
 */
static inline void sensor_imu_backend_accumulate(sensor_imu_backend *imu_backend) {
    if (imu_backend->ops->accumulate != NULL) {
        imu_backend->ops->accumulate(imu_backend);
    }
}

/*
 * Configure and start all sensors. The empty implementation allows
 * subclasses to already start the sensors when it's detected
 */
static inline void sensor_imu_backend_start(sensor_imu_backend *imu_backend) {
    if (imu_backend->ops->start != NULL) {
        imu_backend->ops->start(imu_backend);
    }
}

// get a startup banner to output to the GCS
static inline bool sensor_imu_backend_get_output_banner(sensor_imu_backend *imu_backend, char* banner, uint8_t banner_len){
    if (imu_backend->ops->get_output_banner == NULL) {
        return false;
    }

    return imu_backend->ops->get_output_banner(imu_backend, banner, banner_len);
}

//Returns the Clip Limit
static inline float sensor_imu_backend_get_clip_limit(sensor_imu_backend *imu_backend) { return imu_backend->_clip_limit; }

/*
 * Return the unique identifier for this backend: it's the same for
 * several sensors if the backend registers more gyros/accels
 */
static inline int16_t sensor_imu_backend_get_id(sensor_imu_backend *imu_backend) { return imu_backend->_id; }

// notify of a fifo reset
void sensor_imu_backend_notify_fifo_reset(sensor_imu_backend *imu_backend);

void sensor_imu_backend_rotate_and_correct_accel(sensor_imu_backend *imu_backend, uint8_t instance, Vector3f_t *accel);
void sensor_imu_backend_rotate_and_correct_gyro(sensor_imu_backend *imu_backend, uint8_t instance, Vector3f_t *gyro);

// rotate gyro vector, offset and publish
void sensor_imu_backend_publish_gyro(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *gyro);

// this should be called every time a new gyro raw sample is
// available - be it published or not the sample is raw in the
// sense that it's not filtered yet, but it must be rotated and
// corrected (_rotate_and_correct_gyro)
// The sample_us value must be provided for non-FIFO based
// sensors, and should be set to zero for FIFO based sensors
// uint64_t sample_us,default 0
void sensor_imu_backend_notify_new_gyro_raw_sample(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *accel, uint64_t sample_us);

// rotate accel vector, scale, offset and publish
void sensor_imu_backend_publish_accel(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *accel);

// this should be called every time a new accel raw sample is available -
// be it published or not
// the sample is raw in the sense that it's not filtered yet, but it must
// be rotated and corrected (_rotate_and_correct_accel)
// The sample_us value must be provided for non-FIFO based
// sensors, and should be set to zero for FIFO based sensors
// uint64_t sample_us=0, bool fsync_set=false
void sensor_imu_backend_notify_new_accel_raw_sample(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *accel, uint64_t sample_us, bool fsync_set);

// set the amount of oversamping a accel is doing
void sensor_imu_backend_set_accel_oversampling(sensor_imu_backend *imu_backend, uint8_t instance, uint8_t n);

// set the amount of oversamping a gyro is doing
void sensor_imu_backend_set_gyro_oversampling(sensor_imu_backend *imu_backend, uint8_t instance, uint8_t n);

// indicate the backend is doing sensor-rate sampling for this accel
void sensor_imu_backend_set_accel_sensor_rate_sampling_enabled(sensor_imu_backend *imu_backend, uint8_t instance, bool value);

void sensor_imu_backend_set_gyro_sensor_rate_sampling_enabled(sensor_imu_backend *imu_backend, uint8_t instance, bool value);

// update the sensor rate for FIFO sensors
void sensor_imu_backend_update_sensor_rate(sensor_imu_backend *imu_backend, uint16_t *count, uint32_t *start_us, float *rate_hz);

// return true if the sensors are still converging and sampling rates could change significantly
static inline bool sensor_imu_backend_sensors_converging() { return time_millis() < 30000; }

// set accelerometer max absolute offset for calibration
void sensor_imu_backend_set_accel_max_abs_offset(sensor_imu_backend *imu_backend, uint8_t instance, float offset);

// publish a temperature value
void sensor_imu_backend_publish_temperature(sensor_imu_backend *imu_backend, uint8_t instance, float temperature);

// set accelerometer error_count
void sensor_imu_backend_set_accel_error_count(sensor_imu_backend *imu_backend, uint8_t instance, uint32_t error_count);

// set gyro error_count
void sensor_imu_backend_set_gyro_error_count(sensor_imu_backend *imu_backend, uint8_t instance, uint32_t error_count);

// increment accelerometer error_count
void sensor_imu_backend_inc_accel_error_count(sensor_imu_backend *imu_backend, uint8_t instance);

// increment gyro error_count
void sensor_imu_backend_inc_gyro_error_count(sensor_imu_backend *imu_backend, uint8_t instance);

// return the requested sample rate in Hz
uint16_t sensor_imu_backend_get_loop_rate_hz(sensor_imu_backend *imu_backend);

#if 0
// return the notch filter center in Hz for the sample rate
float sensor_imu_backend_gyro_notch_center_freq_hz(sensor_imu_backend *imu_backend) { return _imu._notch_filter.center_freq_hz(); }

// return the notch filter bandwidth in Hz for the sample rate
float sensor_imu_backend_gyro_notch_bandwidth_hz(sensor_imu_backend *imu_backend) { return _imu._notch_filter.bandwidth_hz(); }

// return the notch filter attenuation in dB for the sample rate
float sensor_imu_backend_gyro_notch_attenuation_dB(sensor_imu_backend *imu_backend) { return _imu._notch_filter.attenuation_dB(); }

bool sensor_imu_backend_gyro_notch_enabled(sensor_imu_backend *imu_backend) { return _imu._notch_filter.enabled(); }

// return the harmonic notch filter center in Hz for the sample rate
float sensor_imu_backend_gyro_harmonic_notch_center_freq_hz(sensor_imu_backend *imu_backend) { return _imu.get_gyro_dynamic_notch_center_freq_hz(); }

// set of harmonic notch current center frequencies
const float* sensor_imu_backend_gyro_harmonic_notch_center_frequencies_hz(sensor_imu_backend *imu_backend) { return _imu.get_gyro_dynamic_notch_center_frequencies_hz(); }

// number of harmonic notch current center frequencies
uint8_t sensor_imu_backend_num_gyro_harmonic_notch_center_frequencies(sensor_imu_backend *imu_backend) { return _imu.get_num_gyro_dynamic_notch_center_frequencies(); }

// return the harmonic notch filter bandwidth in Hz for the sample rate
float sensor_imu_backend_gyro_harmonic_notch_bandwidth_hz(sensor_imu_backend *imu_backend) { return _imu._harmonic_notch_filter.bandwidth_hz(); }

// return the harmonic notch filter attenuation in dB for the sample rate
float sensor_imu_backend_gyro_harmonic_notch_attenuation_dB(sensor_imu_backend *imu_backend) { return _imu._harmonic_notch_filter.attenuation_dB(); }

bool sensor_imu_backend_gyro_harmonic_notch_enabled(sensor_imu_backend *imu_backend) { return _imu._harmonic_notch_filter.enabled(); }
#endif

// common gyro update function for all backends
void sensor_imu_backend_update_gyro(sensor_imu_backend *imu_backend, uint8_t instance);

// common accel update function for all backends
void sensor_imu_backend_update_accel(sensor_imu_backend *imu_backend, uint8_t instance);

void sensor_imu_backend_set_gyro_orientation(uint8_t instance, enum RotationEnum rotation);
void sensor_imu_backend_set_accel_orientation(uint8_t instance, enum RotationEnum rotation);

// increment clipping counted. Used by drivers that do decimation before supplying
// samples to the frontend
void sensor_imu_backend_increment_clip_count(uint8_t instance);

// should fast sampling be enabled on this IMU?
bool sensor_imu_backend_enable_fast_sampling(uint8_t instance);

// if fast sampling is enabled, the rate to use in kHz
uint8_t sensor_imu_backend_get_fast_sampling_rate();

// called by subclass when data is received from the sensor, thus
// at the 'sensor rate'
void sensor_imu_backend_notify_new_accel_sensor_rate_sample(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *accel);
void sensor_imu_backend_notify_new_gyro_sensor_rate_sample(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *gyro);

/*
  notify of a FIFO reset so we don't use bad data to update observed sensor rate
*/
void sensor_imu_backend_notify_accel_fifo_reset(uint8_t instance);
void sensor_imu_backend_notify_gyro_fifo_reset(uint8_t instance);

// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available

// get accelerometer raw sample rate.
float sensor_imu_backend_accel_raw_sample_rate(uint8_t instance);

// set accelerometer raw sample rate;  note that the storage type
// is actually float!
void sensor_imu_backend_set_accel_raw_sample_rate(uint8_t instance, uint16_t rate_hz);

// get gyroscope raw sample rate
float sensor_imu_backend_gyro_raw_sample_rate(uint8_t instance);

// set gyro raw sample rate; note that the storage type is
// actually float!
void sensor_imu_backend_set_gyro_raw_sample_rate(uint8_t instance, uint16_t rate_hz);

void sensor_imu_backend_set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul);
void sensor_imu_backend_set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



